//Misc
#include <stdio.h>
#include <sys/time.h>
#include <iostream>
#include <time.h>
#include <fstream>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "mysvm.cpp"

#include "/usr/local/include/flann/general.h"
#include "/usr/local/include/flann/flann.hpp"

//OpenCV
#include "cv.h"
#include "highgui.h"

//Pcl stuff
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/surface/mls.h"
#include "pcl/io/pcd_io.h"
#include <pcl/features/integral_image_normal.h>
#include <pcl/kdtree/kdtree_flann.h>

//Datatypes
#include "Point.cpp"
#include "FeatureResponse.cpp"
#include "PointCloud.cpp"

#include "PCS.cpp" //Pointcloud samplers (Not used atm)
#include "PairPCS.cpp" //Selects all pairs of points in a pointcloud (Not used atm)

//#include "TF.cpp"//Transformation function class(Not used atm)
//#include "SignPowerTF.cpp"//implementation of Transformation function class (Not used atm)


//Features mostly used by the Integral image based features.
/*
#include "IntegralImageFeatures/Features/Feature.cpp"
#include "IntegralImageFeatures/Features/DummyFeature.cpp"
#include "IntegralImageFeatures/Features/NormalSumFeature.cpp"
#include "IntegralImageFeatures/Features/SimpleValueFeature.cpp"
#include "IntegralImageFeatures/Features/NormPairFeature.cpp"
#include "IntegralImageFeatures/Features/NormAnglePairFeature.cpp"
#include "IntegralImageFeatures/Features/AVGPosFeature.cpp"
#include "IntegralImageFeatures/Features/HistFeature.cpp"
#include "IntegralImageFeatures/Features/CompositFeature.cpp"
#include "IntegralImageFeatures/Features/SphericalCoordinateFeature.cpp"

//Point cloud samplers
#include "IntegralImageFeatures/PCS/IIPCS.cpp"
#include "IntegralImageFeatures/PCS/IIPairPCS.cpp"
#include "IntegralImageFeatures/PCS/IIPointPCS.cpp"
*/

//
#include "IntegralImageFeatures/TransformationFunction/TF.cpp"
#include "IntegralImageFeatures/TransformationFunction/SignPowerTF.cpp"

//#include "IntegralImageFeatures/new_Features/TF.cpp"

#include "IntegralImageFeatures/new_Features/Feature.cpp"
#include "IntegralImageFeatures/new_Features/NormalSumFeature.cpp"
#include "IntegralImageFeatures/new_Features/PosSumFeature.cpp"
#include "IntegralImageFeatures/new_Features/ValueFeature.cpp"
#include "IntegralImageFeatures/new_Features/SphericalCoordinateFeature.cpp"
#include "IntegralImageFeatures/new_Features/SphericalCoordinateDifferenceFeature.cpp"
#include "IntegralImageFeatures/new_Features/CompositFeature.cpp"
#include "IntegralImageFeatures/new_Features/HistogramFeature.cpp"


//Integral image based features
#include "IntegralImageFeatures/IntegralImageFeature.cpp"
#include "IntegralImageFeatures/IntegralImageComposit.cpp"
//#include "IntegralImageFeatures/IntegralImageTest.cpp"
//#include "IntegralImageFeatures/IntegralImageHist.cpp"
#include "IntegralImageFeatures/IntegralImageValue.cpp"
#include "IntegralImageFeatures/IntegralImageGeneralFeature.cpp"



//Actualy regressors
#include "Classifiers/Classefier.cpp"
#include "Classifiers/SVMClassefier.cpp"
//#include "Classefiers/SVMPrune.cpp"
//#include "Classefiers/GPBASEClassefier.cpp"
//#include "Classefiers/GPBOOSTClassefier.cpp"
//#include "Classefiers/SVMBOOSTClassefier.cpp"

#define  BUFSIZE 256 //Temporary, wont be in final version
#define mydefx 20//Temporary, wont be in final version
#define mydefy 20//Temporary, wont be in final version
#define mydefstep 10//Temporary, wont be in final version


//TODO place for size

pcl::IntegralImageNormalEstimation normalEstimator;
pcl::IntegralImageNormalEstimation * normalEstimator_pointer = 0;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
pcl::PointCloud<pcl::PointXYZRGB> cloud;



void show_matrix(float ** mat,int sx, int sy);
void show_rgb_matrix(float ** r,float ** g,float ** b,int sx, int sy);
void rotate_point(Point * point);
//void setup_rotation(float rx, float ry, float rz);


//Size of frame that we are working on at the moment
const int width_resolution = 640;
const int height_resolution = 480;
//Dataset for Training
char * data_path= (char *)"../data/dataset_dummy2/";


/*
 *  This Quickselect routine is based on the algorithm described in
 *  "Numerical recipes in C", Second Edition,
 *  Cambridge University Press, 1992, Section 8.5, ISBN 0-521-43108-5
 *  This code by Nicolas Devillard - 1998. Public domain.
 */


#define ELEM_SWAP(a,b) { register float t=(a);(a)=(b);(b)=t; }

float quick_select(float * arr, int n) 
{
    int low, high ;
    int median;
    int middle, ll, hh;

    low = 0 ; high = n-1 ; median = (low + high) / 2;
    for (;;) {
        if (high <= low)
            return arr[median] ;

        if (high == low + 1) {  
            if (arr[low] > arr[high])
                ELEM_SWAP(arr[low], arr[high]) ;
            return arr[median] ;
        }


    middle = (low + high) / 2;
    if (arr[middle] > arr[high])    ELEM_SWAP(arr[middle], arr[high]) ;
    if (arr[low] > arr[high])       ELEM_SWAP(arr[low], arr[high]) ;
    if (arr[middle] > arr[low])     ELEM_SWAP(arr[middle], arr[low]) ;


    ELEM_SWAP(arr[middle], arr[low+1]) ;


    ll = low + 1;
    hh = high;
    for (;;) {
        do ll++; while (arr[low] > arr[ll]) ;
        do hh--; while (arr[hh]  > arr[low]) ;

        if (hh < ll)
        break;

        ELEM_SWAP(arr[ll], arr[hh]) ;
    }


    ELEM_SWAP(arr[low], arr[hh]) ;


    if (hh <= median)
        low = ll;
        if (hh >= median)
        high = hh - 1;
    }
}


#undef ELEM_SWAP

	using namespace std;

/**
 * Store an array of responses in a file
 * @path_to_save: 		where to save the file
 * @responses_to_store:	Responses to store(the data)
 * @responses_in_array:	Number of responses in array to store.
 * @save_number: 		Identification number for the file(appended at the back of the filename)
 **/
void store_responses_in_array(char * path_to_save, FeatureResponse ** responses_to_save, int responses_in_array, int save_number){
	FeatureResponse ** responses_to_delete = responses_to_save;
	char filename[256];
	sprintf(filename, "%sfeature_file_nr_%d.feature",path_to_save, save_number);
	printf("Storing feature in: %s\n",filename);


	 fstream filestr;
	 filestr.open(filename,fstream::trunc | fstream::in | fstream::out | fstream::binary);
	 filestr.write((char*)(&responses_in_array),sizeof(int));
	 for(int i = 0; i < responses_in_array; i++)
	 {
		 filestr.write((char*)(&(responses_to_delete[i]->length)),sizeof(int));
		 filestr.write((char*)(responses_to_delete[i]->response),(responses_to_delete[i]->length)*sizeof(float));
	 }

	 filestr.flush();
	 filestr.close();
}

/**
 * Gets responses from a .feature file
 * @filename: Name of the file containing the features
 * @nr_features: Parameter that tells you how many features that was read from the file
 **/
FeatureResponse ** get_responses_from_file(char * filename, int& nr_features){


	 fstream filestr;
	 int length;
	 char * buffer;
	 filestr.open(filename,fstream::in | fstream::binary);
	 filestr.seekg (0, ios::end);
	 length = filestr.tellg();
	 filestr.seekg (0, ios::beg);
	 buffer = new char [length];
	 filestr.read (buffer,length);
	 filestr.close();
	 int* bi = (int*)buffer;
	 float* bf = (float*)buffer;
	 nr_features = bi[0];
	 FeatureResponse ** ret = new FeatureResponse*[nr_features];
	 printf("nr_features = %i\n",nr_features);
	 int read_index = 1;
	 int counter = 0;
	 while(read_index < length/4)
	 {
		 int length_of_feature = bi[read_index];
		 float * responses = new float[length_of_feature];
		 for(int i = 0; i < length_of_feature;i++)
		 {
			 responses[i]=bf[i+read_index+1];
		 }
		 read_index += length_of_feature+1;
		 ret[counter] = new FeatureResponse(responses,length_of_feature);
		 counter++;
	 }
	 delete buffer;
	 return ret;
}

/*
 * Deletes responses from a response array.
 */
void delete_responses_in_array(FeatureResponse ** responses_to_delete, int responses_in_array){
	for(int i = 0; i < responses_in_array; i++)
	{
		delete responses_to_delete[i];
	}
}

void deletePointMatrix(Point *** image_pointcloud)
{
	for(int i = 0;i<width_resolution;i++)
	{
		for(int j = 0;j < height_resolution;j++)
		{
			delete image_pointcloud[i][j];
		}
		delete image_pointcloud[i];
	}
	delete image_pointcloud;
}

float weight(float distance)
{
	float v = pow(distance,-1.2);
	float maximum = 80.5;
	if(v > maximum){v = maximum;}
	v /= maximum;
	float minimum = 0.40f;
	if(v < minimum){v = minimum;}
	return v;
}

/*
 Gets a Point matrix from a frame in the datafolder
 @frame_number: Number of the frame
 @path: Path to the data
 */
Point *** getPointMatrix(int frame_number, char * path, char * item)
{
    struct timeval start, end, start_inside, end_inside;
    long mtime, seconds, useconds;
    mtime = 0;
gettimeofday(&start, NULL);


	char filename[256];
	IplImage * rgb_image;
	IplImage * depth_image;
	IplImage * mask_image;
	const int ImageCenterX = width_resolution >> 1;
	const int ImageCenterY = height_resolution >> 1;
	const float rgb_focal_length_VGA = 525;//Magical focal length assumption
	const float scale_3D_const = 0.001f / rgb_focal_length_VGA;

	Point *** image_pointcloud = new Point**[width_resolution];
	for(int i = 0;i<width_resolution;i++)
	{
		image_pointcloud[i] = new Point*[height_resolution];
		for(int j = 0;j < height_resolution;j++)
		{
			image_pointcloud[i][j] = new Point();
		}
	}

	float tmp_norms[width_resolution][height_resolution][3];
	bool first_time = true;
	
	//RGB
	sprintf(filename, "%sframe%d_rgb.bmp",path, frame_number);
	rgb_image = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);
	//printf("\nfilename: %s\n",filename);

	//Depth
	sprintf(filename, "%sframe%d_depth.bmp",path, frame_number);
	depth_image = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);
	//printf("filename: %s\n",filename);
	//Mask
	if(item != NULL)
	{
		sprintf(filename, "%s%s/%s_frame%d_mask.bmp",path,item,item, frame_number);
		mask_image = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);
		//printf("filename: %s\n",filename);
	}
	
	if(rgb_image==NULL)
	{
		//printf("Time to end...\n");
		return image_pointcloud;
	}
	
	//Populate point matrixes
	image_pointcloud[0][0]->norm_z=1;
	image_pointcloud[0][0]->norm_y=1;
	image_pointcloud[0][0]->norm_x=1;
	for(int i = 0; i < width_resolution;i++)
	{
		for(int j = 0; j < height_resolution;j++)
		{
			int distance_from_cam = 0;
			int part1 = 0xFF&depth_image->imageData[3*(j*width_resolution+i)+0];
			int part2 = 0xFF&depth_image->imageData[3*(j*width_resolution+i)+1];
			distance_from_cam = (part1<<8)+part2;
			int b = 0xFF&rgb_image->imageData[3*(j*width_resolution+i)+0];
			int g = 0xFF&rgb_image->imageData[3*(j*width_resolution+i)+1];
			int r = 0xFF&rgb_image->imageData[3*(j*width_resolution+i)+2];
			float x = (j -  ImageCenterX) * distance_from_cam * scale_3D_const;
			float y = (i - ImageCenterY) * distance_from_cam * scale_3D_const;
			float z = distance_from_cam * 0.001 ; // because values are in mm
			image_pointcloud[i][j]->r = r;
			image_pointcloud[i][j]->g = g;
			image_pointcloud[i][j]->b = b;
			image_pointcloud[i][j]->x = x;
			image_pointcloud[i][j]->y = y;
			image_pointcloud[i][j]->z = z;
			rotate_point(image_pointcloud[i][j]);
			bool is_object = false;
			if(item!=NULL)
			{
				is_object = (0xFF&(mask_image->imageData[3*(j*width_resolution+i)+0]))>0;
			}
			image_pointcloud[i][j]->is_object = is_object;
			image_pointcloud[i][j]->response = 0;
		}
	}
	if(rgb_image!=NULL)cvReleaseImage(&rgb_image);
	if(depth_image!=NULL)cvReleaseImage(&depth_image);
	if(mask_image!=NULL && item != NULL)cvReleaseImage(&mask_image);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	
	cloud->width  = width_resolution;
	cloud->height = height_resolution;
	cloud->points.resize (cloud->width*cloud->height);
	for(int i = 0; i < width_resolution;i++)
	{
		for(int j = 0; j < height_resolution;j++)
		{
			pcl::PointXYZ& pt = (*cloud)(i,j);
			if(image_pointcloud[i][j]->z!=0)
			{
				(*cloud)(i,j).x = image_pointcloud[i][j]->x;
				(*cloud)(i,j).y = image_pointcloud[i][j]->y;
				(*cloud)(i,j).z = image_pointcloud[i][j]->z;
			}
			else{
				pt.x = pt.y = pt.z = bad_point;
			}
		}
	}
/*
	// Create a KD-Tree
	pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
	tree->setInputCloud (cloud);

	pcl::MovingLeastSquares<pcl::PointXYZ,pcl::Normal> mls;
	pcl::PointCloud<pcl::PointXYZ> mls_points,cloud_final;

	// Optionally, a pointer to a cloud can be provided, to be set by MLS
	pcl::PointCloud<pcl::Normal>::Ptr mls_normals (new pcl::PointCloud<pcl::Normal> ());
	mls.setOutputNormals (mls_normals);
		
	// Set parameters
	mls.setInputCloud (cloud);
	mls.setPolynomialFit (false);
	//mls.setPolynomialOrder(81)
	mls.setSearchMethod (tree);
	mls.setSearchRadius (0.03);//mls.setSearchRadius (0.03);

	// Reconstruct
	mls.reconstruct (mls_points);

	pcl::PointCloud<pcl::PointNormal> mls_cloud;
	pcl::concatenateFields (mls_points, *mls_normals, mls_cloud);

	printf("pre cloud %d, %d\n" ,cloud->width, cloud->height);
	printf("final cloud %d, %d\n", cloud_final.width, cloud_final.height);
	printf("mls cloud %d, %d\n", mls_cloud.width, mls_cloud.height);

	pcl::io::savePCDFile("pre2.pcd",*cloud);
	pcl::io::savePCDFile("mls.pcd",mls_cloud);
	
	for(int i = 0; i < width_resolution;i++)
	{
		for(int j = 0; j < height_resolution;j++)
		{
			(*cloud)(i,j).x = mls_cloud(i*height_resolution + j,1).x;
			(*cloud)(i,j).y = mls_cloud(i*height_resolution + j,1).y;
			(*cloud)(i,j).z = mls_cloud(i*height_resolution + j,1).z;
		}
	}
*/

	normalEstimator_pointer = new pcl::IntegralImageNormalEstimation();
	normalEstimator_pointer->setInputData(reinterpret_cast<float*>(&(cloud->points[0])),cloud->width, cloud->height,
	   3, sizeof(cloud->points[0])/sizeof(float), (sizeof(cloud->points[0])/sizeof(float))*cloud->width, 10.0f);

	printf("changed.......");
	int rsize = 21;
	normalEstimator_pointer->setRectSize(rsize, rsize);
	
	for(int i = 0; i < width_resolution;i++)
	{
		for(int j = 0; j < height_resolution;j++)
		{
			if(i>(1+(rsize/2))&&j>(1+(rsize/2))){
				pcl::Normal normal = normalEstimator_pointer->compute(i, j);
				image_pointcloud[i][j]->norm_x=normal.normal_x;
				image_pointcloud[i][j]->norm_y=normal.normal_y;
				image_pointcloud[i][j]->norm_z=normal.normal_z;
			}
			else{
				image_pointcloud[i][j]->norm_x=0;
				image_pointcloud[i][j]->norm_y=0;
				image_pointcloud[i][j]->norm_z=0;
			}
		}
	}
	for(int i = 0; i < width_resolution;i++)
	{
		for(int j = 0; j < height_resolution;j++)
		{
			tmp_norms[i][j][0] = image_pointcloud[i][j]->norm_x;
			tmp_norms[i][j][1] = image_pointcloud[i][j]->norm_y; //= tmp_norms[i][j][1];
			tmp_norms[i][j][2] = image_pointcloud[i][j]->norm_z; //= tmp_norms[i][j][2];
			float r = sqrt(tmp_norms[i][j][0]*tmp_norms[i][j][0] + tmp_norms[i][j][1]*tmp_norms[i][j][1]+tmp_norms[i][j][2]*tmp_norms[i][j][2]);
			if(r>0){
				image_pointcloud[i][j]->norm_azimuth = acos(tmp_norms[i][j][2]/r);
				if(tmp_norms[i][j][0]!=0)
					{
					image_pointcloud[i][j]->norm_zenith = atan(tmp_norms[i][j][1]/tmp_norms[i][j][0]);
					}
				else{
					image_pointcloud[i][j]->norm_zenith = 0;
					}
			}
		}
	}
	delete normalEstimator_pointer;

	gettimeofday(&end, NULL);
	seconds  = end.tv_sec  - start.tv_sec;
	useconds = end.tv_usec - start.tv_usec;
	mtime = seconds*1000000  + useconds;
	if(useconds<0)
	{
		useconds += 1000000;
		seconds--;
	}
    	bool ** tested = new bool *[640];
	for(int i = 0; i < 640; i++)
	{
		tested[i]=new bool[480];
		for(int j = 0; j < 480;j++)
		{
			tested[i][j]=false;
		}
	}
	Point *** components = new Point**[15];
	int * points_in_component = new int[15];
	int nr_components = 0;
	//TODO: USE LISTS! Fix memleeks
	int nr_in_list = 0;
	int * int_x = new int[1000000];
	int * int_y = new int[1000000];

	float ** print_matrix = new float *[640];
	float ** print_matrix_r = new float *[640];
	float ** print_matrix_g = new float *[640];
	float ** print_matrix_b = new float *[640];
	for(int i = 0; i < 640; i++)
	{
		print_matrix[i]=new float[480];
		print_matrix_r[i]=new float[480];
		print_matrix_g[i]=new float[480];
		print_matrix_b[i]=new float[480];
		for(int j = 0; j < 480;j++)
		{
			image_pointcloud[i][j]->is_valid = is_valid(image_pointcloud[i][j]);
			print_matrix[i][j]=image_pointcloud[i][j]->distance_to_object;//is_object;
		}
	}
//Finds connected components
	for(int i = 0; i < 640; i++)
	{
		for(int j = 0; j < 480;j++)
		{
			if(!tested[i][j])
			{

				if(image_pointcloud[i][j]->is_object && image_pointcloud[i][j]->is_valid)
				{
					components[nr_components]=new Point*[1000000];

					nr_in_list = 1;
					int k = 0;
					int_x[k] = i;
					int_y[k] = j;
					components[nr_components][k]=image_pointcloud[int_x[k]][int_y[k]];
					tested[int_x[k]][int_y[k]]=true;

					while(k < nr_in_list)
					{
						int tmpx= int_x[k]+1;
						int tmpy= int_y[k];
						if(tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0){
							if(!tested[tmpx][tmpy] && image_pointcloud[tmpx][tmpy]->is_object && image_pointcloud[tmpx][tmpy]->is_valid)
							{
								components[nr_components][nr_in_list] = image_pointcloud[tmpx][tmpy];
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy]=true;
								nr_in_list++;
							}
						}
						tmpx= int_x[k]-1;
						tmpy= int_y[k];
						if(tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0){
							if(!tested[tmpx][tmpy] && image_pointcloud[tmpx][tmpy]->is_object && image_pointcloud[tmpx][tmpy]->is_valid)
							{
								components[nr_components][nr_in_list] = image_pointcloud[tmpx][tmpy];
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy]=true;
								nr_in_list++;
							}
						}
						tmpx= int_x[k];
						tmpy= int_y[k]+1;
						if(tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0){
							if(!tested[tmpx][tmpy] && image_pointcloud[tmpx][tmpy]->is_object && image_pointcloud[tmpx][tmpy]->is_valid)
							{
								components[nr_components][nr_in_list] = image_pointcloud[tmpx][tmpy];
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy]=true;
								nr_in_list++;
							}
						}
						tmpx= int_x[k];
						tmpy= int_y[k]-1;
						if(tmpx < 640 && tmpx >= 0 && tmpy < 480 && tmpy >= 0){
							if(!tested[tmpx][tmpy] && image_pointcloud[tmpx][tmpy]->is_object && image_pointcloud[tmpx][tmpy]->is_valid)
							{
								components[nr_components][nr_in_list] = image_pointcloud[tmpx][tmpy];
								int_x[nr_in_list] = tmpx;
								int_y[nr_in_list] = tmpy;
								tested[tmpx][tmpy]=true;
								nr_in_list++;
							}
						}
						k++;
					}
					points_in_component[nr_components]=nr_in_list;
					nr_components++;
				}
				tested[i][j]=true;
			}
		}
	}
	float * median_x = new float[nr_components];
	float * median_y = new float[nr_components];
	float * median_z = new float[nr_components];
	for(int i = 0; i < nr_components;i++)
	{
		float * medx = new float[points_in_component[i]];
		float * medy = new float[points_in_component[i]];
		float * medz = new float[points_in_component[i]];

		for(int j = 0; j < points_in_component[i];j++)
		{
			medx[j] = components[i][j]->x;
			medy[j] = components[i][j]->y;
			medz[j] = components[i][j]->z;
		}

		median_x[i] = quick_select(medx,points_in_component[i]);
		median_y[i] = quick_select(medy,points_in_component[i]);
		median_z[i] = quick_select(medz,points_in_component[i]);

		printf("%10f , %10f , %10f\n", median_x[i],median_y[i],median_z[i]);

		delete medx;
		delete medy;
		delete medz;
		delete components[i];
	}
	delete components;
	delete points_in_component;
	delete int_x;
	delete int_y;
	for(int i = 0; i < 640; i++)
	{
		for(int j = 0; j < 480;j++)
		{
			for(int k = 0; k < nr_components;k++)
			{
				float xsquare = (image_pointcloud[i][j]->x - median_x[k])*(image_pointcloud[i][j]->x - median_x[k]);
				float ysquare = (image_pointcloud[i][j]->y - median_y[k])*(image_pointcloud[i][j]->y - median_y[k]);
				float zsquare = (image_pointcloud[i][j]->z - median_z[k])*(image_pointcloud[i][j]->z - median_z[k]);
				float dist = xsquare + ysquare + zsquare;
				if(dist < image_pointcloud[i][j]->distance_to_object)
				{
					image_pointcloud[i][j]->distance_to_object = dist;
				}
			}
		}
	}

//for(float height = 0.62; height < 1; height += 0.005)
//{
//printf("height = %f\n",height);
	for(int i = 0; i < 640; i++)
	{
		for(int j = 0; j < 480;j++)
		{
			//print_matrix_r[i][j] = (image_pointcloud[i][j]->r+image_pointcloud[i][j]->g+image_pointcloud[i][j]->b)/3;
			//print_matrix_g[i][j] = (image_pointcloud[i][j]->r+image_pointcloud[i][j]->g+image_pointcloud[i][j]->b)/3;
			//print_matrix_b[i][j] = (image_pointcloud[i][j]->r+image_pointcloud[i][j]->g+image_pointcloud[i][j]->b)/3;
			if(image_pointcloud[i][j]->is_valid && !image_pointcloud[i][j]->is_object){
			
				print_matrix[i][j]   = weight(image_pointcloud[i][j]->distance_to_object);
				print_matrix_r[i][j] = (image_pointcloud[i][j]->norm_x + 1)*128;
				print_matrix_g[i][j] = (image_pointcloud[i][j]->norm_y + 1)*128;
				print_matrix_b[i][j] = (image_pointcloud[i][j]->norm_z + 1)*128;

				/*if(fabs(0.67-image_pointcloud[i][j]->x)<0.03f){
					print_matrix_r[i][j] = (image_pointcloud[i][j]->norm_x + 1)*128;
					print_matrix_g[i][j] = (image_pointcloud[i][j]->norm_y + 1)*128;
					print_matrix_b[i][j] = (image_pointcloud[i][j]->norm_z + 1)*128;
				}else{
					print_matrix_r[i][j] = (image_pointcloud[i][j]->r+image_pointcloud[i][j]->g+image_pointcloud[i][j]->b)/3;
					print_matrix_g[i][j] = (image_pointcloud[i][j]->r+image_pointcloud[i][j]->g+image_pointcloud[i][j]->b)/3;
					print_matrix_b[i][j] = (image_pointcloud[i][j]->r+image_pointcloud[i][j]->g+image_pointcloud[i][j]->b)/3;
				}*/

			}
			else{print_matrix[i][j] = -123;}
		}
	}
	show_matrix(print_matrix,640, 480);
	show_rgb_matrix(print_matrix_r,print_matrix_g,print_matrix_b,640, 480);
//}

	for(int i = 0; i < 640; i++)
	{
		delete print_matrix[i];
		delete print_matrix_r[i];
		delete print_matrix_g[i];
		delete print_matrix_b[i];
		delete tested[i];
	}
	delete print_matrix;
	delete print_matrix_r;
	delete print_matrix_g;
	delete print_matrix_b;
	delete tested;

	return image_pointcloud;
}

/*float angle_in_rad = -0.4;
float rot_matrix[][3] = { 
	{ cos(angle_in_rad), 	0, 	-sin(angle_in_rad)},
	{ 0, 			1, 	0},
	{ sin(angle_in_rad), 	0,	cos(angle_in_rad)},
};*/

float rot_matrix[][3] = { 
{ 0.85717, 0, 0.51504},
{ 0, 1, 0},
{ -0.51504, 0, 0.85717},
};

void rotate_point(Point * point)
{
	float x = point->x * rot_matrix[0][0]+point->y * rot_matrix[0][1]+point->z * rot_matrix[0][2];
	float y = point->x * rot_matrix[1][0]+point->y * rot_matrix[1][1]+point->z * rot_matrix[1][2];
	float z = point->x * rot_matrix[2][0]+point->y * rot_matrix[2][1]+point->z * rot_matrix[2][2];

	point->x = x;
	point->y = y;
	point->z = z;
}

/*
 Checks if a point is valid to use within calculations.
 Used to make sure its not a point with bad data given from the kinect.
 */
inline bool is_valid(Point* p){
	return  !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z) &&!((p->norm_x==0)&&(p->norm_y==0)&&(p->norm_z==0));//;
}

/*
 Checks if a point is valid to use within calculations and is part of an object.
 Used to make sure its not a point with bad data given from the kinect.
 */
inline bool is_valid_and_object(Point* p){
	return (p->is_object) && !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z)&&!(p->norm_x==0)&&!(p->norm_y==0)&&!(p->norm_z==0);//;
}

/*
 Checks if a point is valid to use within calculations and makes sure it is not a part of an object.
 Used to make sure its not a point with bad data given from the kinect.
 */
inline bool is_valid_and_not_object(Point* p){
	return !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z) &&!((p->norm_x==0)&&(p->norm_y==0)&&(p->norm_z==0));//!(p->is_object) && !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z)&&!(p->norm_x==0)&&!(p->norm_y==0)&&!(p->norm_z==0);//;
}

/**
 * Evaluates the likelihood density of an image containing the object
 * that the area can contain the object.
 * @cl: Classefier used to evaluate a patch
 * @iifeature: Feature used for evaluation.
 * @frame_number: Number of rame in the datafolder to be evaluated
 * @path: Path to the data
 **/
void evaluate_image(Classefier * cl,IntegralImageFeature * iifeature, int frame_number, char * path)
{
	struct timeval start, end, start_inside, end_inside;
	long mtime, seconds, useconds;
	mtime = 0;
	float ** mat = new float*[640];
	for(int i = 0; i < 640;i++)
	{
		mat[i]=new float[480];
	}

	float ** mat_div = new float*[640];
	for(int i = 0; i < 640;i++)
	{
		mat_div[i]=new float[480];
	}


	//printf("Evaluating %i \n", frame_number);

	for(int i = 0; i<640;i++)
	{
		for(int j = 0; j<480;j++)
		{
			mat_div[i][j]=0;
			mat[i][j]=0;
		}
	}

	float ** resp_holder = new float*[width_resolution];
	float ** counter_holder = new float*[width_resolution];
	for(int i = 0; i<width_resolution;i++)
	{
		resp_holder[i] = new float[height_resolution];
		counter_holder[i] = new float[height_resolution];
	}
	gettimeofday(&start, NULL);

	Point *** image_pointcloud = getPointMatrix(frame_number,path,NULL);//TODO: takes 0.5s... optimization needed 
	int nrtest = 0;

    //iifeature = new IntegralImageGeneralFeature(image_pointcloud, 0,0);
    iifeature->reset(image_pointcloud);

	int start_patchsize_width = mydefx;
	int end_patchsize_width = mydefx;
	int patchsize_width_step =3;
	gettimeofday(&start_inside, NULL);

	for(int patchsize_width = start_patchsize_width;patchsize_width <= end_patchsize_width;patchsize_width += patchsize_width_step)
	{
		int start_patchsize_height = mydefy;
		int end_patchsize_height = mydefy;
		int patchsize_height_step =1;
		for(int patchsize_height = start_patchsize_height;patchsize_height <= end_patchsize_height;patchsize_height += patchsize_height_step)
		{
			for(int i = 0; i<width_resolution;i++)
			{
				for(int j = 0; j < height_resolution;j++)
				{
					resp_holder[i][j] = 0;
					counter_holder[i][j] = 0;
				}
			}
			//printf("New patchsize: %i x %i\n",patchsize_width,patchsize_height);
			//For all wanted patches of this size
			int wstep = 1;//mydefstep;
			int maxw = width_resolution-patchsize_width;
			for(int w = 1; w < maxw;w+=wstep)
			{
				int hstep = 1;//mydefstep;
				int maxh = height_resolution-patchsize_height;
				for(int h = 1; h < maxh;h+=hstep)
				{	

					int wt = w;
					int ht = h;
					FeatureResponse * feature_response;
					if(iifeature->acceptable(w,h,patchsize_width,patchsize_height, (patchsize_width*patchsize_height)*0.6f))
					{

						feature_response 		= iifeature->getResponse(w,h,patchsize_width,patchsize_height);
						FeatureResponse * answer = cl->evaluate(feature_response);

						float value = answer->response[0];
						//printf("%i , %i -> %f\n",w,h,value);

						if(isnan(value))
						{
							resp_holder[wt][ht]=0;
							counter_holder[wt][ht] = 0;
						}
						else{
							resp_holder[wt][ht]=value;counter_holder[wt][ht] = 1;
						}
						nrtest++;
						//TODO: Ersätt med integralbild!
						if(!isnan(value))
						{
							for(int i = w; i < w+patchsize_width;i++)
							{
								for(int j = h; j < h+patchsize_height;j++)
								{
									mat[i][j]+=value;
									mat_div[i][j]++;
								}
							}
						}
						delete feature_response;
						delete answer;

					}

				}
			}
		}
	}
	gettimeofday(&end_inside, NULL);
	seconds  = end_inside.tv_sec  - start_inside.tv_sec;
	useconds = end_inside.tv_usec - start_inside.tv_usec;
	if(useconds<0)
	{
		useconds += 1000000;
		seconds--;
	}
	mtime += seconds*1000000  + useconds;

	for(int i = 0; i<640;i++)
	{
		for(int j = 0; j<480;j++)
		{
			if(mat_div[i][j] == 0){
				mat[i][j]=-123;
			}
			else{
				mat[i][j]/=mat_div[i][j];
			}
		}
	}

	for(int i = 0; i<640;i++)
	{
		delete resp_holder[i];
		delete counter_holder[i];

		for(int j = 0; j<480;j++)
		{
			delete image_pointcloud[i][j];
		}
		delete image_pointcloud[i];
	}
	delete image_pointcloud;
	delete resp_holder;
	delete counter_holder;


	gettimeofday(&end, NULL);
	seconds  = end.tv_sec  - start.tv_sec;
	useconds = end.tv_usec - start.tv_usec;
	//printf("Elapsed time adding total : %f sec\n", (float)(((double)mtime)/((double)1000000)));
	mtime = seconds*1000000  + useconds;
	if(useconds<0)
	{
		useconds += 1000000;
		seconds--;
	}
	//printf("Elapsed time total : %ld.%ld sec, #tested = %i\n", seconds,useconds,nrtest);
	show_matrix(mat,640, 480);

}

IplImage * work_image = 0;
void show_matrix(float ** mat,int sx, int sy)
{
	float min = 10000000;
	float max = -1000000;
	for(int i = 0; i < sx;i++)
	{
		for(int j = 0; j < sy;j++)
		{
			if(mat[i][j] != -123)
			{
				if(mat[i][j]<min)
				{
					min = mat[i][j];
				}else if(mat[i][j]>max)
				{
					max = mat[i][j];
				}
			}
		}
	}

	float mul = 255.0f/(max - min);
	if(work_image != 0)cvReleaseImage(&work_image);
	work_image=cvCreateImage(cvSize(sx,sy),IPL_DEPTH_8U,3);
	for(int i = 0; i < sx;i++)
	{
		for(int j = 0; j < sy;j++)
		{
			if(mat[i][j] != -123)
			{
				int val = (mat[i][j]-min)*mul;
				work_image->imageData[3*(j*sx+i)+0] = val;
				work_image->imageData[3*(j*sx+i)+1] = val;
				work_image->imageData[3*(j*sx+i)+2] = val;
			}else{
				work_image->imageData[3*(j*sx+i)+0] = 0;
				work_image->imageData[3*(j*sx+i)+1] = 0;
				work_image->imageData[3*(j*sx+i)+2] = 0;
			}
		}
	}
	cvNamedWindow("black and white image", CV_WINDOW_AUTOSIZE);
	cvShowImage("black and white image", work_image);
	cvWaitKey(100);
}

IplImage * work_image_rgb = 0;
void show_rgb_matrix(float ** r,float ** g,float ** b,int sx, int sy)
{
	float min = 10000000;
	float max = -1000000;
	for(int i = 0; i < sx;i++)
	{
		for(int j = 0; j < sy;j++)
		{
			if(r[i][j] != -123)
			{
				if(r[i][j]<min)
				{
					min = r[i][j];
				}else if(r[i][j]>max)
				{
					max = r[i][j];
				}
			}
			if(g[i][j] != -123)
			{
				if(g[i][j]<min)
				{
					min = g[i][j];
				}else if(g[i][j]>max)
				{
					max = g[i][j];
				}
			}

			if(b[i][j] != -123)
			{
				if(b[i][j]<min)
				{
					min = b[i][j];
				}else if(b[i][j]>max)
				{
					max = b[i][j];
				}
			}
		}
	}

	float mul = 255.0f/(max - min);
	if(work_image_rgb != 0)cvReleaseImage(&work_image_rgb);
	work_image_rgb=cvCreateImage(cvSize(sx,sy),IPL_DEPTH_8U,3);
	for(int i = 0; i < sx;i++)
	{
		for(int j = 0; j < sy;j++)
		{
			if(b[i][j] != -123)
			{
				int valb = (b[i][j]-min)*mul;
				work_image_rgb->imageData[3*(j*sx+i)+0] = valb;

			}else{
				work_image_rgb->imageData[3*(j*sx+i)+0] = 0;
			}

			if(g[i][j] != -123)
			{
				int valg = (g[i][j]-min)*mul;
				work_image_rgb->imageData[3*(j*sx+i)+1] = valg;

			}else{
				work_image_rgb->imageData[3*(j*sx+i)+1] = 0;
			}

			if(r[i][j] != -123)
			{
				int valr = (r[i][j]-min)*mul;
				work_image_rgb->imageData[3*(j*sx+i)+2] = valr;

			}else{
				work_image_rgb->imageData[3*(j*sx+i)+2] = 0;
			}
		}
	}
	cvNamedWindow("rgb image", CV_WINDOW_AUTOSIZE);
	cvShowImage("rgb image", work_image_rgb);
	cvWaitKey(100);
}

/**
 * Function used to make a testingfunction for regressor evaluation
 **/
FeatureResponse * testFunc(FeatureResponse * input)
{
    float * tmp = new float[1];
    float sum = 0;
    for(int i = 0; i < input->length;i++)
    {
        //sum +=  input->response[i];
    }
    sum /= sqrt(input->length)/2;
    tmp[0]=sin(input->response[0] * 10)*cos(input->response[1] *2)*0.5f +0.5f+sum;
    //printf("tmp[0] = %f \n",tmp[0]);
    return new FeatureResponse(tmp,1);
}


void test_ml()
{
    //int responses_saved = 0;//responses_per_file;
    int nr_trainingdata = 10000000;
    int nr_testdata = 15000;
    FeatureResponse ** training_data = new FeatureResponse*[nr_trainingdata];
    FeatureResponse ** training_values = new FeatureResponse*[nr_trainingdata];
    float x1_min  = 0;
    float x2_min  = 0;
    float x1_max  = 1;
    float x2_max  = 1;
    srand(1);
    int ndims;
for(ndims = 10; ndims < 2000; ndims *= 2)
{
	printf("NDIMS = %i\n",ndims);
	    for(int i = 0; i < nr_trainingdata;i++)
	    {
		float * tmp = new float[ndims];
		for(int dim = 0; dim < ndims; dim++)
		{
		    float x1 = rand()%100000;///((float)RAND_MAX);
		    x1 = x1/100000.0f;
		    x1 = x1*(x1_max - x1_min) + x1_min;
		    tmp[dim]=x1;
		}
		training_data[i]=new FeatureResponse(tmp,2);
		training_values[i]=testFunc(training_data[i]);
		float noise = rand()%100000;///((float)RAND_MAX);
		noise = noise/100000.0f;
		noise -= 0.5;
		noise *=0.5;
		noise *= noise*noise;
		//training_values[i]->response[0] += noise;
		//printf("%f \n",training_values[i]->response[0]);
	    }
	    printf("trainingdata setup\n");
	    FeatureResponse ** test_data = new FeatureResponse*[nr_testdata];
	    FeatureResponse ** test_values = new FeatureResponse*[nr_testdata];
	    for(int i = 0; i < nr_testdata;i++)
	    {
		float x1 = rand()%100000;///((float)RAND_MAX);
		x1 = x1/100000.0f;
		x1 = x1*(x1_max - x1_min) + x1_min;
		float x2 = rand()%100000;///((float)RAND_MAX);
		x2 = x2/100000.0f;
		x2 = x2*(x2_max - x2_min) + x2_min;
	       
		float * tmp = new float[2];
		tmp[0]=x1;
		tmp[1]=x2;
		test_data[i]=new FeatureResponse(tmp,2);
		test_values[i]=testFunc(test_data[i]);
	    }
	    printf("testdata setup\n");
	    int nr_classefiers = 3;
	    Classefier ** classefiers = new Classefier*[nr_classefiers];
	    char ** classefier_names = new char*[nr_classefiers];
	    int * max_td = new int[nr_classefiers];
	    int * min_td = new int[nr_classefiers];

	    classefiers[0] = new SVMClassefier();
	    classefier_names[0] = (char *)"SVMClassefier";
	    max_td[0]=2000;
	    min_td[0]=10;
	    for(int j = 0; j < nr_classefiers;j++)
	    {
		printf("---%s--- \n",classefier_names[j]);
		printf("#Trainingdata | Training Time |   Test time | mean square error | mean error\n");
		for(int nr_tr = min_td[j];nr_tr <= max_td[j];nr_tr*=2.0f)
		{
		    printf("%13i | ",nr_tr);
		    struct timeval start, end;
		    long mtime, seconds, useconds;
		    gettimeofday(&start, NULL);
		    classefiers[j]->train(training_values,training_data,NULL,nr_tr);
		    gettimeofday(&end, NULL);
		        seconds  = end.tv_sec  - start.tv_sec;
		    useconds = end.tv_usec - start.tv_usec;
		    mtime = seconds*1000000  + useconds;

		    printf("%12fs | ",((float)mtime)/((float)1000000));
		    gettimeofday(&start, NULL);
		    FeatureResponse ** batchresponses = classefiers[j]->batchEvaluate(test_data,nr_testdata);
		    gettimeofday(&end, NULL);
		        seconds  = end.tv_sec  - start.tv_sec;
		    useconds = end.tv_usec - start.tv_usec;
		    mtime = seconds*1000000  + useconds;
		    printf("%10fs | ",((float)mtime)/((float)1000000));
		    float mse = 0;
		    float me = 0;
		    for(int i = 0; i < nr_testdata;i++)
		    {
		        FeatureResponse * resp2 = batchresponses[i]; //gpbaseclassifier->evaluate(test_data[i]);
		        //printf("Expected = %f , got = %f ,",test_values[i]->response[0],resp2->response[0]);
		        float diff = fabs(resp2->response[0] - test_values[i]->response[0]);
		       //printf("diff = %f \n",diff);
		        mse+=diff*diff;
		        me +=diff;
		        delete resp2;
		    }
		    mse/=(float)nr_testdata;
		    me/=(float)nr_testdata;
		    printf("%17f | %10f\n",mse,me);
		    delete batchresponses;
		}
	    }

	}
}

void cluster(FeatureResponse ** target, FeatureResponse ** data,FeatureResponse **& out_target,FeatureResponse **& out_data,float ** data_weights,int nr_data,int nr_clusters)
{
	cv::Mat samples = cv::Mat_<float>(nr_data, data[0]->length);
	for(int i = 0; i < nr_data;  i++)
		{for(int j = 0; j < data[0]->length;j++)
			{samples.at<float>(i, j)=data[i]->response[j];}}

	cv::Mat labels, centers;
	double compactness = cv::kmeans(samples, nr_clusters , labels, cv::TermCriteria(), 4, cv::KMEANS_PP_CENTERS, &centers);

	float * weights = new float[nr_clusters];
	float * value = new float[nr_clusters];
	for(int i = 0; i < nr_clusters;i++){weights[i]= 0; value[i]=0;}
	for(int i = 0; i < labels.rows; ++i)
	{
		weights[labels.at<int>(0, i)]++;
		value[labels.at<int>(0, i)]+=target[i]->response[0];
	}
	for(int i = 0; i < nr_clusters;i++){value[i]/=(float)weights[i];}

	out_target = new FeatureResponse*[nr_clusters];
	out_data = new FeatureResponse*[nr_clusters];
	//printf("CLUSTERS = \n");
	//printf("-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=\n");
	for(int i = 0; i < centers.rows; ++i)
	{
		float * tmp_out_target = new float[1];
		tmp_out_target[0]=value[i];
		out_target[i] = new FeatureResponse(tmp_out_target,1);


		float * tmp_out_data = new float[centers.cols];
		out_data[i]  = new FeatureResponse(tmp_out_data,centers.cols);

		//printf("%f <-- ",tmp_out_target[0]);
		for(int j = 0; j < centers.cols; j++){
			tmp_out_data[j]=centers.at<float>(j, i);
		//	if(tmp_out_data[j]>0){printf(" ");}
		//	printf("%f, ",tmp_out_data[j]);
		}
		//printf("\n");
	}
printf("\n-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+\n");
for(int k = 0; k < nr_clusters;k++)
{
	float ** tmp_holder = new float*[data[0]->length];
	for(int dimcounter = 0;dimcounter < data[0]->length;dimcounter++)
	{
		tmp_holder[dimcounter] = new float[(int)weights[k]];
	}
	int printed = 0;
	//printf("----------------------------\nlabel = %i , weight = %i, value = %f\n",k,weights[k],value[k]);
	for(int i = 0; i < nr_data ;  i++){
		if(labels.at<int>(0, i) == k)
		{
			for(int j = 0; j < data[0]->length;j++){
				float * tmparray = tmp_holder[j];
				tmparray[printed]=data[i]->response[j];
			}
			printed++;
			if(printed < 0)
			{
				printf("%f <- ",target[i]->response[0]);
				for(int j = 0; j < data[0]->length;j++){
					if(data[i]->response[j] >= 0){printf(" ");}
					printf("%f , " , data[i]->response[j]);
				}
				printf("\n");
			}
		}
	}
	printf("label = %5i , weight = %5i->%5f, value = %5f <------[",k,(int)weights[k],((float)(weights[k]*nr_clusters))/(float)nr_data,value[k]);
	weights[k] = ((float)(weights[k]*nr_clusters))/(float)nr_data,value[k];
	for(int j = 0; j < data[0]->length;j++){
		float * tmparray = tmp_holder[j];
		float thismedian = quick_select(tmparray, weights[k]);
		if(thismedian >=0){printf(" ");}
		printf("%.5f, ",thismedian);
		out_data[k]->response[j]=thismedian;
		delete tmparray;
	}
	printf("]\n");
	delete tmp_holder;

}
printf("-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+-+=+\n");
*data_weights = weights;
	//delete weights;
	delete value;
}
/*
 * Main function of the program
 */
int main(int argc, char** argv){
	printf("---------INPUT----------\n");
	char ** data_paths = new char*[100];
	char * item = NULL;
	int nr_data_paths = 0;
	int ii;
	for(ii = 1; ii < argc;ii++)
	{
		printf("input: %s\n",argv[ii]);
		if(!strcmp(argv[ii], "--item"))
		{
			item = argv[++ii];
			printf("item set to = %s\n",item);
			continue;
		}
		if(!strcmp(argv[ii], "--data"))
		{
			++ii;
			printf("input: %s\n",argv[ii]);
			fstream filestr;
			filestr.open(argv[ii],fstream::in | fstream::binary);
			filestr.seekg (0, ios::end);
			int length = filestr.tellg();
		 	char * buffer = new char [length];
			filestr.seekg (0, ios::beg);
			filestr.read (buffer,length);
			filestr.close();
			int this_str_length = 1;
			int last_str_start = 0;
			for(int j = 0; j < length;j++)
			{
				if(buffer[j]=='\n')
				{	
					this_str_length++;
					char * str = new char[this_str_length];
					for(int q = 0; q < this_str_length-2;q++)
					{
						str[q]=buffer[q+last_str_start];
					}
					str[this_str_length-2]= 0;
					str[this_str_length-1]= 0;
					data_paths[nr_data_paths] = str;
					this_str_length = 0;
					last_str_start  = j+1;
					nr_data_paths++;
				}
				this_str_length++;
			}
			delete buffer;
			continue;
		}
	}
	printf("Nr Data filepath: %i\n",nr_data_paths);
	for(int i = 0; i < nr_data_paths;i++)
	{
		printf("Data filepath %i = %s\n",i,data_paths[i]);
	}

	printf("---------------------------------\nInitializing v1.21\n");
	cloud.width  = width_resolution;
	cloud.height = height_resolution;
	cloud.points.resize (cloud.width*cloud.height);

	const int responses_per_file = 10000000;
	int save_number = 0;
	int responses_saved = 0;
	FeatureResponse ** responses_to_save_now = new FeatureResponse*[responses_per_file];
	FeatureResponse ** values_to_save_now = new FeatureResponse*[responses_per_file];


	Feature ** featurebase = new Feature*[6];

	featurebase[0] = new PosSumFeature();
	featurebase[1] = new NormalSumFeature();

	int * binsi =  new int[2];	binsi[0] = 9;				binsi[1]=9;
	float * mini = new float[2];	mini[0]  = 0;				mini[1] =0;
	float * maxi = new float[2];	maxi[0]  = 0.1;			maxi[1] =1;
	TF ** transf = new TF*[2]; 	transf[0] = new SignPowerTF(0.5f); 	transf[1] = new SignPowerTF(0.7f); 
	featurebase[2] = new HistogramFeature(new SphericalCoordinateDifferenceFeature(),binsi,mini,maxi,transf);//NULL,NULL,NULL,NULL);//);
	featurebase[3] = new CompositFeature(featurebase,2);
	featurebase[4] = new SphericalCoordinateDifferenceFeature();
	featurebase[5] = new SphericalCoordinateFeature();


	IntegralImageFeature ** iifeats_for_iifeature = new IntegralImageFeature*[6];
	iifeats_for_iifeature[0]=new IntegralImageGeneralFeature(NULL,featurebase[0],0,0);//SphericalCoordinateFeature
	iifeats_for_iifeature[1]=new IntegralImageGeneralFeature(NULL,featurebase[1],0,0);//NormalSumFeature
	iifeats_for_iifeature[2]=new IntegralImageGeneralFeature(NULL,featurebase[2],0,0);//HistogramFeature
	iifeats_for_iifeature[3]=new IntegralImageGeneralFeature(NULL,featurebase[3],0,0);//CompositFeature
	iifeats_for_iifeature[4]=new IntegralImageGeneralFeature(NULL,featurebase[4],0,0);//SphericalCoordinateDifferenceFeature
	iifeats_for_iifeature[5]=new IntegralImageGeneralFeature(NULL,featurebase[5],0,0);//PosSumFeature
	
	IntegralImageFeature * iifeature = iifeats_for_iifeature[3];

	Feature * featureValue = new ValueFeature();
	//IntegralImageFeature * iivalue = new IntegralImageGeneralFeature(NULL,featureValue,0,0);
	IntegralImageFeature * iivalue = new IntegralImageValue(NULL,0,0);
	char * current_data_path;
	for(int current_data_dir_number = 0;current_data_dir_number < nr_data_paths; current_data_dir_number++)
	{
		current_data_path = data_paths[current_data_dir_number];
		//printf("#################################################################\nReading directory: %s\n",current_data_path);
		for(int frame_number = 1;frame_number <= 300;frame_number = frame_number+10)
		{
			//printf("\n### Getting data from: %s and frame number = %i\n",current_data_path,frame_number);
			Point *** image_pointcloud;
			image_pointcloud = getPointMatrix(frame_number,current_data_path,item);

			//TODO IIfeature
			iifeature->reset(image_pointcloud);
			iivalue->reset(image_pointcloud);
			
			int start_patchsize_width = mydefx;
			int end_patchsize_width = mydefx;
			int patchsize_width_step =1;
			for(int patchsize_width = start_patchsize_width;patchsize_width <= end_patchsize_width;patchsize_width += patchsize_width_step)
			{
				int start_patchsize_height = mydefy;
				int end_patchsize_height = mydefy;
				int patchsize_height_step =1;
				for(int patchsize_height = start_patchsize_height;patchsize_height <= end_patchsize_height;patchsize_height += patchsize_height_step)
				{
					int wstep = mydefstep;
					int maxw = width_resolution-patchsize_width;
					for(int w = 1; w < maxw;w+=wstep)
					{
						int hstep = mydefstep;
						int maxh = height_resolution-patchsize_height;
						for(int h = 1; h < maxh;h+=hstep)
						{								
							FeatureResponse * feature_response;
							FeatureResponse * feature_value;
							if(iifeature->acceptable(w,h,patchsize_width,patchsize_height, (patchsize_width*patchsize_height)*0.01f))
							{
								feature_response 	= iifeature->getResponse(w,h,patchsize_width,patchsize_height);
								/*if(feature_response->response[0]==0){
									int topx = w; int topy = h;int sizex = patchsize_width;int sizey = patchsize_height;
									float divi = iifeature->counterimg[topx+sizex][topy+sizey]+iifeature->counterimg[topx][topy]-iifeature->counterimg[topx+sizex][topy]-iifeature->counterimg[topx][topy+sizey];
									printf("<->-<->-<-> %.5i,%.5i = divi = %.3i -> [%f,%f,%f,%f]\n",w,h,(int)divi,feature_response->response[0],feature_response->response[1],feature_response->response[2],feature_response->response[3]);
									exit(0);
								}*/
								feature_value 		= iivalue->getResponse(w,h,patchsize_width,patchsize_height);
								responses_to_save_now[responses_saved] = feature_response;
								values_to_save_now[responses_saved] = feature_value;

								responses_saved++;

								if(responses_saved>=responses_per_file)
								{
									printf("saveing featureset: %i \n",save_number);
									save_number++;
									store_responses_in_array((char *)"./store/responses/", responses_to_save_now, responses_per_file, save_number);
									store_responses_in_array((char *)"./store/values/", values_to_save_now, responses_per_file, save_number);
									responses_saved = 0;
									delete_responses_in_array(responses_to_save_now, responses_per_file);
									delete_responses_in_array(values_to_save_now, responses_per_file);
								}
							}
						}
					}
				}
			}

			deletePointMatrix(image_pointcloud);
			//printf("### Nr in featureset: %i \n",responses_saved);		
		}
	}

	//Add PCA,LDA or somehting like that as a postprosessing effect on the trainingdata

	int buckets =2;
	int * distribution = new int[buckets];
	int * distribution_backup = new int[buckets];
	FeatureResponse *** distribution_responses= new FeatureResponse**[buckets];
	FeatureResponse *** distribution_values= new FeatureResponse**[buckets];

	for(int i = 0; i < buckets; i++)
	{
		distribution[i] = 0;
		distribution_backup[i] = 0;
	}
	for(int i = 0; i < responses_saved; i++)
	{
		float floatval = values_to_save_now[i]->response[0];
		//int cval = floatval * buckets;
		int cval;
		if(floatval==0){cval = 0;}
		else{cval = 1;}
		if(cval >= buckets){cval = buckets-1;}
		else if(cval < 0){cval = 0;}
		distribution[cval]++;
		distribution_backup[cval]++;
	}
	printf("Distribution = [");

	for(int i = 0; i < buckets; i++)
	{
		printf("%f ",(float)distribution[i]/(float)responses_saved);
	}
	printf("]\n");

	for(int i = 0; i < responses_saved; i++)//Random order
	{
		int swap_pos =  rand()%(responses_saved-1);
		FeatureResponse * tmp_fr = responses_to_save_now[i];
		FeatureResponse * tmp_vfr = values_to_save_now[i];
		responses_to_save_now[i] = responses_to_save_now[swap_pos];
		values_to_save_now[i] = values_to_save_now[swap_pos];
		responses_to_save_now[swap_pos] = tmp_fr;
		values_to_save_now[swap_pos] = tmp_vfr;
	}

	for(int i = 0; i < buckets; i++)
	{
		distribution_responses[i] = new FeatureResponse*[distribution[i]];
		distribution_values[i] = new FeatureResponse*[distribution[i]];
	}

	for(int i = 0; i < responses_saved; i++)
	{
		float floatval = values_to_save_now[i]->response[0];
		int cval = floatval * buckets;
		if(floatval==0){cval = 0;}
		else{cval = 1;}
		if(cval >= buckets){cval = buckets-1;}
		else if(cval < 0){cval = 0;}
		//if(i < 200){printf("floatval = %f -> \n",cval,floatval);}
		distribution[cval]--;
		distribution_responses[cval][distribution[cval]]=responses_to_save_now[i];
		distribution_values[cval][distribution[cval]]=values_to_save_now[i];
	}

	FeatureResponse** even_distribution_values = new FeatureResponse*[responses_saved];
	FeatureResponse** even_distribution_responses = new FeatureResponse*[responses_saved];

	int even_response_counter = 0;
	int current_bucket = 0;

	while(even_response_counter < responses_saved)
	{
		if(distribution_backup[current_bucket]>0) //how many items there's in this bucket still
		{
			distribution_backup[current_bucket]--;
			even_distribution_values[even_response_counter] = distribution_values[current_bucket][distribution_backup[current_bucket]];
			even_distribution_responses[even_response_counter] = distribution_responses[current_bucket][distribution_backup[current_bucket]];

			//Output for debugging
			/*if(even_response_counter<10 && (even_response_counter % 2 == 0)){
				printf("%f<-[",even_distribution_values[even_response_counter]->response[0]);
				printf("%f",even_distribution_responses[even_response_counter]->response[0]);
				for(int count = 1; count < even_distribution_responses[even_response_counter]->length;count++)
				{
					printf(", %f",even_distribution_responses[even_response_counter]->response[count]);
				}
				printf("]\n");
			}
			if(even_response_counter<20 && even_response_counter>10 && (even_response_counter % 2 == 1)){
				printf("%f<-[",even_distribution_values[even_response_counter]->response[0]);
				printf("%f",even_distribution_responses[even_response_counter]->response[0]);
				for(int count = 1; count < even_distribution_responses[even_response_counter]->length;count++)
				{
					printf(", %f",even_distribution_responses[even_response_counter]->response[count]);
				}
				printf("]\n");
			}*/

			even_response_counter++;
		}
		current_bucket++;
		if(current_bucket>=buckets)
		{
			current_bucket = 0;
		}
	}

	SVMClassefier * classifier = new SVMClassefier();


	for(int nr_feat_for_train = 37; nr_feat_for_train < 51/*responses_saved*/;nr_feat_for_train*=2){
		struct timeval start, end;
		long mtime, seconds, useconds;
		mtime = 0;

		printf("Classefier started with %i features and ",nr_feat_for_train);
		gettimeofday(&start, NULL);
		
		int nr_clusters = nr_feat_for_train;//100;
		FeatureResponse ** out_values;
		FeatureResponse ** out_responses;
		float ** data_weights = new float*;
		printf("starting clustering\n");
		cluster(even_distribution_values,even_distribution_responses,out_values,out_responses,data_weights,responses_saved,nr_clusters);
		printf("done clustering\n");
		classifier->train(out_values,out_responses,*data_weights,nr_clusters);
		
		gettimeofday(&end, NULL);
		seconds  = end.tv_sec  - start.tv_sec;
		useconds = end.tv_usec - start.tv_usec;
		mtime = seconds*1000000  + useconds;
		if(useconds<0)
		{
			useconds += 1000000;
			seconds--;
		}
		printf("Elapsed time training total : %f sec\n", (float)(((double)mtime)/((double)1000000)));
		evaluate_image(classifier,iifeature,20,current_data_path);
		delete data_weights;
	}
	for(int i = 20;i<60;i++){
		evaluate_image(classifier,iifeature,i,current_data_path);
		cvWaitKey(0);
	}
	//evaluate_image(classifier,iifeature,1,current_data_path);
	//cvWaitKey(0);

	printf("Done\n");
	return 0;
}

